#include "phys_engine.h"
namespace phys_engine
{
	// _stick 构造函数有两种重载：
	//1. XY_INIT  a=x1, b=y1, c=x2, d=y2
	//2. LA_INIT  a=xc, b=yc, c=length, d=angle
	_stick::_stick(int init,double input_mass,double input_a,double input_b,
				double input_c,double input_d,int input_show_style)
	{
		_mass=input_mass;
		_show_style=input_show_style;
		if(init==XY_INIT)
		{
			_xc=(input_a+input_c)/2;
			_yc=(input_b+input_d)/2;
			_length=point_distance(input_a,input_b,input_c,input_d);
			_angle=calc_angle(input_a,input_b,input_c,input_d);
		}
		else
		{
			_xc=input_a;
			_yc=input_b;
			_length=input_c;
			_angle=input_d;
		}
	}

	simulate_point _stick::end_point_1(void) const // l+minus
	{
		return calc_point(_xc,_yc,_angle,-_length/2.0);
	}

	simulate_point _stick::end_point_2(void) const // l+plus
	{
		return calc_point(_xc,_yc,_angle,_length/2.0);
	}

	void _stick::_force(double _effx,double _effy,double _divx,double _divy)
	{
		_accx+=_divx/_mass;
		_accy+=_divy/_mass;
		_angacc+=calc_angacc(_effx-_xc,_effy-_yc,_divx,_divy,_mass*_length*_length/12.0);
	}

	void _stick::_move(void)
	{
		// vel
		_velx+=_accx*tick;
		_vely+=_accy*tick;
		_angvel+=_angacc*tick;

		// mov
		_xc+=(_velx*tick+_accx*tick*tick/2);
		_yc+=(_vely*tick+_accy*tick*tick/2);
		_angle+=(_angvel*tick+_angacc*tick*tick/2);

		// acc
		_accx=_accy=_angacc=0;

	}

	simulate_point stick_end_point_1(_stick *_s)
	{
		return _s->end_point_1();
	}

	simulate_point stick_end_point_2(_stick *_s)
	{
		return _s->end_point_2();
	}

	void stick_force(_stick *_s,double _effx,double _effy,double _divx,double _divy)
	{
		_s->_force(_effx,_effy,_divx,_divy);
	}

	void stick_move(_stick *_s)
	{
		_s->_move();
	}
	
}
